#include "offboard.hpp"

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
}

void local_odom_callback(const nav_msgs::Odometry & current_msg)
{
    current_point.x= current_msg.pose.pose.position.x;
    current_point.y= current_msg.pose.pose.position.y;
    // current_point.z= current_msg.pose.pose.position.z;
    current_point.z= current_point_z;

    body_vel.x = current_msg.twist.twist.linear.x;
    body_vel.y = current_msg.twist.twist.linear.y;
    body_vel.z = current_msg.twist.twist.linear.z;

    current_angle.x= current_msg.pose.pose.orientation.x;
    current_angle.y= current_msg.pose.pose.orientation.y;
    current_angle.z= current_msg.pose.pose.orientation.z;
    current_angle.w= current_msg.pose.pose.orientation.w;
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg.pose.pose.orientation.w, current_msg.pose.pose.orientation.x,
                                        current_msg.pose.pose.orientation.y, current_msg.pose.pose.orientation.z);
    local_odom_flag = true;
}

void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel1_value = rcvalue.channels[0]; //roll
    channel2_value = rcvalue.channels[1]; //pitch
    channel3_value = rcvalue.channels[2]; //thrust
    channel4_value = rcvalue.channels[3]; //yaw
    channel5_value = rcvalue.channels[4];
    channel6_value = rcvalue.channels[5];
    channel7_value = rcvalue.channels[6];
    channel8_value = rcvalue.channels[7];
}

void planWaypoint_callback(const quadrotor_msgs::PositionCommand &msg){
    planned_point.x= msg.position.x;
    planned_point.y= msg.position.y;
    planned_point.z= msg.position.z;
    planned_velocity.x = msg.velocity.x;
    planned_velocity.y = msg.velocity.y;
    planned_velocity.z = msg.velocity.z;
    planned_yaw_angle = msg.yaw;
    planned_yaw_dot = msg.yaw_dot;
    planned_update_flag = true;
}

void velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vel.x = msg.twist.linear.x;
    current_vel.y = msg.twist.linear.y;
    current_vel.z = msg.twist.linear.z;
}


void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    if(print_state_cnt>20)
    {
        //print state per second
        printFSMExecState();
        print_state_cnt = 0;
    }
    print_state_cnt++;
    
    switch (exec_state) { 
    case INIT: {
        if (!local_odom_flag) {
            break;
        }
        if(use_sitl)
        {
            changeFSMExecState(TAKEOFF, "offboard_fsm");
        }
        else
        {
            changeFSMExecState(CRUISE, "offboard_fsm");
        }
    break;
    }
    case TAKEOFF: {
        if( current_state.mode != "OFFBOARD" &&
        (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent){
                ROS_INFO("Offboard enabled");
            }
            last_offboard_request = ros::Time::now();
        } 
        else {
            if( !current_state.armed &&
                (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed.");
                }
                last_offboard_request = ros::Time::now();
            }
        }
        velocity_expected.x = 0.0;
        velocity_expected.y = 0.0;
        velocity_expected.z = 0.5;
        vel_command_send(velocity_expected, 0);
        if(current_point.z>1.5)
        {
            changeFSMExecState(CRUISE, "offboard_fsm");
        }
    break;
    }

    case CRUISE: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        test_cnt++;
        set_cruise_state();

        if(current_state.mode == "OFFBOARD")
        {
            changeFSMExecState(PLANNER, "offboard_fsm");    
        }
    break;
    }

    case PLANNER: {
        max_vel_beishu = 0.5;
        std_msgs::Float32MultiArray maxvel_pub;
        maxvel_pub.data.push_back(max_vel_beishu);
        max_vel_pub.publish(maxvel_pub);
        simple_goal_planner(planner_target_point_x, planner_target_point_y, planner_target_point_z, 0, false);
        // double distance_to_planner = sqrt((planner_target_point_x-current_point.x)*(planner_target_point_x-current_point.x)
        //                                         +(planner_target_point_y-current_point.y)*(planner_target_point_y-current_point.y));
        // if(distance_to_planner<0.5&&abs(current_vel.x)<0.2&&abs(current_vel.y)<0.2&&abs(current_vel.z)<0.2)
        // {
        //     changeFSMExecState(GO_PIPELINE, "offboard_fsm");
        // }
    break;
    }

    }
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file
    nh.param<double>("mission_number",mission_number,false);
    nh.param<bool>("use_sitl",use_sitl,false);
    nh.param<double>("max_vel_beishu",max_vel_beishu,1);

    nh.param<double>("planner_start_point_x",planner_start_point_x,1);
    nh.param<double>("planner_start_point_y",planner_start_point_y,1);
    nh.param<double>("planner_start_point_z",planner_start_point_z,1);

    nh.param<double>("planner_target_point_x",planner_target_point_x,1);
    nh.param<double>("planner_target_point_y",planner_target_point_y,1);
    nh.param<double>("planner_target_point_z",planner_target_point_z,1);


    //Subscriber
    altitude_sub = nh.subscribe("/mavros/altitude",10,altitude_callback);
    planWaypointsub = nh.subscribe("/planning/pos_cmd", 10, planWaypoint_callback);
    velocitysub     = nh.subscribe("/mavros/local_position/velocity_local",10,velocity_callback);
    local_odom_sub = nh.subscribe("/Odometry",10,local_odom_callback);
    rcin_sub = nh.subscribe("/mavros/rc/in",10,rcin_callback);
    state_sub = nh.subscribe("/mavros/state", 10, state_cb);


    //Publisher
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros/setpoint_velocity/cmd_vel", 10);
    simple_goal_pub_ = nh.advertise<geometry_msgs::PoseStamped>("/move_base_simple/goal", 10);
    max_vel_pub = nh.advertise<std_msgs::Float32MultiArray>("/zhou/max_vel",10);

    control_plot = nh.advertise<std_msgs::Float32MultiArray>("/plot/controller",10);


    //Service
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("mavros/setpoint_velocity/mav_frame");

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.05), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(100);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"------------>wait for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;
    setmavframe.request.mav_frame = 8;
    last_offboard_request = ros::Time::now();
    //set mavframe to body_ned 8
    if(set_vechile_ned.call(setmavframe) && setmavframe.response.success)
    {
        std::cout<<"------------>setmavframe success..."<<std::endl;
    }
    else{
        std::cout<<"------------>setmavframe failed..."<<std::endl;
    }
    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

void vel_command_send(geometry_msgs::Point vel, double yaw_rate)
{
    vel_command_cnt++;
    geometry_msgs::Vector3 linear_temp;
    geometry_msgs::Vector3 angular_temp;

    geometry_msgs::TwistStamped msg_vel_pub;
    linear_temp.x = vel.x;
    linear_temp.y = vel.y;
    linear_temp.z = vel.z;
    
    angular_temp.x = 0;
    angular_temp.y = 0;
    angular_temp.z = yaw_rate;
    msg_vel_pub.header.stamp = ros::Time::now();
    msg_vel_pub.header.seq=vel_command_cnt;
    msg_vel_pub.twist.linear=linear_temp;
    msg_vel_pub.twist.angular=angular_temp;
    local_vel_pub.publish(msg_vel_pub);
}


void changeFSMExecState(FSM_STATE new_state, std::string pos_call) {

  int    pre_s        = int(exec_state);
  exec_state         = new_state;
  std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
  std::cout << "state: " + state_str[int(exec_state)] << std::endl;
}


void position_pid_control(geometry_msgs::Point current_set_point,geometry_msgs::Point current_local_point,float velocity_limit,float target_yaw, float dead_zone, bool ifplan)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, yaw_rate_i = 0.0, yaw_rate_d = 0.0, vz = 0;
    float roll = 0.0, pitch = 0.0;
    geometry_msgs::Point error_pos;
    float dt = ros::Time::now().toSec()-last_calc_time;
    if(dt>1.0) //dt too long maybe need init dt
    {
        last_calc_time = ros::Time::now().toSec();
        return;
    }
    //position control  mode & 0x01
    if (1) {
        //calculate velocity, P control
        
        error_pos.x = current_set_point.x - current_local_point.x;
        error_pos.y = current_set_point.y - current_local_point.y;
        vxp = P_pos * error_pos.x;
        vyp = P_pos * error_pos.y;
        vxd = -(D_pos * current_vel.x);
        vyd = -(D_pos * current_vel.y);

        if (abs(error_pos.x) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.x += error_pos.x*dt;
        } else {
            error_pos_integrated.x = 0.0;
        }
        if (abs(error_pos.y) <= POS_I_DEAD_ZONE) {
            error_pos_integrated.y += error_pos.y*dt;
        } else {
            error_pos_integrated.y = 0.0;
        }
        
        error_pos_integrated.x = limit((double) error_pos_integrated.x, max_vel_beishu*1.0 / I_pos);
        error_pos_integrated.y = limit((double) error_pos_integrated.y, max_vel_beishu*1.0 / I_pos);
        
        vxi = I_pos * error_pos_integrated.x;
        vyi = I_pos * error_pos_integrated.y;
        
        if(ifplan)
        {
            vx = (vxp + vxi + vxd)+planned_velocity.x;
            vy = (vyp + vyi + vyd)+planned_velocity.y;
        }
        else
        {
            vx = vxp;
            vy = vyp;
        }
        
        //limit the speed
        // vx_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).x;
        // vy_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).y;
        vx_lim = limit(vx, velocity_limit);
        vy_lim = limit(vy, velocity_limit);
        //ROS_INFO("vx_exp vy_exp %f %f",vx,vy);

        error_pos_last = error_pos;
    }
    //yaw control  mode & 0x02
    if (1) {
        //use the flight controller yaw in default
        
        float current_yaw = curr_plane_angle.z;
        //Drone turns at the smallest angle
        float error_yaw = (target_yaw - current_yaw) * rad2deg;
        if (error_yaw < -180)
            error_yaw += 360;
        else if (error_yaw > 180)
            error_yaw -= 360;
        yaw_rate_p = P_yaw * error_yaw;
        yaw_rate_d = D_yaw * (error_yaw - error_yaw_last);
        error_yaw_integrated += error_yaw;
        if (I_yaw > 0.0001) {
            error_yaw_integrated = limit(error_yaw_integrated, YAW_I_LIMIT / I_yaw);
        }
        yaw_rate_i = I_yaw * error_yaw_integrated;
        if (abs(error_yaw) <= YAW_DEAD_ZONE) {
            yaw_rate_p = 0.0;
            yaw_rate_i = 0.0;
            yaw_rate_d = 0.0;
            error_yaw_integrated = 0.0;
        }
        yaw_rate = (yaw_rate_p + yaw_rate_i + yaw_rate_d);
        if (abs(error_yaw) >= YAW_RATE_LIMIT) {
            yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
        } 
        error_yaw_last = error_yaw;
    }

    //height control  mode & 0x04
    if (1) {
        float deltaz = current_local_point.z - current_set_point.z;
        if (fabs(deltaz) >= HEIGHT_DEAD_ZONE) {
            vz = P_z * (current_set_point.z - current_local_point.z);
        } else {
            vz = 0.0;
        }
    }
    velocity_expected.x = vx_lim * cos(curr_plane_angle.z) + vy_lim * sin(curr_plane_angle.z);
    velocity_expected.y = vy_lim * cos(curr_plane_angle.z) - vx_lim * sin(curr_plane_angle.z);
    velocity_expected.z = vz;
    attitude_expect.z = yaw_rate * deg2rad;
    last_calc_time = ros::Time::now().toSec();

    //control plot 
    std_msgs::Float32MultiArray plot_msg;
    plot_msg.data.push_back(error_pos.x);
    plot_msg.data.push_back(error_pos.y);
    plot_msg.data.push_back(vx_lim);
    plot_msg.data.push_back(vy_lim);

    control_plot.publish(plot_msg);
}

void set_cruise_state()
{
    velocity_expected.x = 0.0;
    velocity_expected.y = 0.0;
    velocity_expected.z = 0.0;
    vel_command_send(velocity_expected, 0);
}

void waypoints_no_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, double yuzhi)
{
    geometry_msgs::Point this_point;
    this_point.x = waypoints_plan.waypoints[pipline_waypoint_num].x_lat;
    this_point.y = waypoints_plan.waypoints[pipline_waypoint_num].y_long;
    this_point.z = waypoints_plan.waypoints[pipline_waypoint_num].z_alt;

    position_pid_control(this_point,current_point,max_vel_beishu,yaw,0,false);
    if(velocity_expected.z>0.3)
    {
        velocity_expected.z=0.3;
    }
    vel_command_send(velocity_expected, attitude_expect.z);

    double distance_to_go_nextpoint = sqrt((waypoints_plan.waypoints[pipline_waypoint_num].x_lat-current_point.x)*(waypoints_plan.waypoints[pipline_waypoint_num].x_lat-current_point.x)
                                                +(waypoints_plan.waypoints[pipline_waypoint_num].y_long-current_point.y)*(waypoints_plan.waypoints[pipline_waypoint_num].y_long-current_point.y));
    if(distance_to_go_nextpoint<yuzhi)
    {
        if(pipline_waypoint_num<waypoints_plan.waypoints.size()-1)
        {
            pipline_waypoint_num++;
        }
        
    }


}

void waypoints_planner(mavros_msgs::WaypointList waypoints_plan, double yaw, bool specified_yaw)
{
    if(pub_planner_waypoints_once)
    {
        geometry_msgs::PoseStamped goal_msg;
        goal_msg.pose.position.x = waypoints_plan.waypoints[waypoint_exec_num].x_lat;
        goal_msg.pose.position.y = waypoints_plan.waypoints[waypoint_exec_num].y_long;
        goal_msg.pose.position.z = waypoints_plan.waypoints[waypoint_exec_num].z_alt;
        simple_goal_pub_.publish(goal_msg);
        last_path_send = ros::Time::now();
        pub_planner_waypoints_once = false;
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
        if(ros::Time::now()-last_path_send>ros::Duration(2.0))
        {
            pub_planner_waypoints_once = true;
        }
        
    }
    else
    {
        if(specified_yaw)
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,yaw,0,true);
            vel_command_send(velocity_expected, attitude_expect.z);
            // vel_command_send(velocity_expected, 0);
        }
        else
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,0,0,true);
            // vel_command_send(velocity_expected, attitude_expect.z);
            vel_command_send(velocity_expected, 0);
        }
    }
    planned_update_flag = false;

    double distance_to_go_nextpoint = sqrt((waypoints_plan.waypoints[waypoint_exec_num].x_lat-current_point.x)*(waypoints_plan.waypoints[waypoint_exec_num].x_lat-current_point.x)
                                                +(waypoints_plan.waypoints[waypoint_exec_num].y_long-current_point.y)*(waypoints_plan.waypoints[waypoint_exec_num].y_long-current_point.y));
    if(distance_to_go_nextpoint<0.5&&ros::Time::now()-last_path_send>ros::Duration(1.0))
    {
        if(waypoint_exec_num<waypoints_plan.waypoints.size()-1)
        {
            waypoint_exec_num++;
            pub_planner_waypoints_once = true;
        }
        
    }


}

void simple_goal_planner(double target_x, double target_y, double target_z, double yaw, bool specified_yaw)
{
    if(pub_planner_goal_once)
    {
        geometry_msgs::PoseStamped goal_msg;
        goal_msg.pose.position.x = target_x;
        goal_msg.pose.position.y = target_y;
        goal_msg.pose.position.z = target_z;
        simple_goal_pub_.publish(goal_msg);
        last_pub_goal_time = ros::Time::now();
        pub_planner_goal_once = false;
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
        if(ros::Time::now()-last_pub_goal_time>ros::Duration(0.5))
        {
            planner_cnt++;
            pub_planner_goal_once = true;
            std::cout<<"------>planner_cnt: "<<planner_cnt<<std::endl;
        }
        
    }
    else
    {
        if(specified_yaw)
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,yaw,0,true);
            vel_command_send(velocity_expected, attitude_expect.z);
            // vel_command_send(velocity_expected, 0);
        }
        else
        {
            position_pid_control(planned_point,current_point,max_vel_beishu+0.5,0,0,true);
            vel_command_send(velocity_expected, attitude_expect.z);
            // vel_command_send(velocity_expected, 0);
        }
    }
    planned_update_flag = false;
}

void continuously_goal_planner(double target_x, double target_y, double target_z)
{
    double dist_to_goal = sqrt((target_x-current_point.x)*(target_x-current_point.x)+(target_y-current_point.y)*(target_y-current_point.y));
    if(ros::Time::now()-last_pub_tracking_time>ros::Duration(0.5)&&dist_to_goal>0.5)
    {
        geometry_msgs::PoseStamped goal_msg;
        goal_msg.pose.position.x = target_x;
        goal_msg.pose.position.y = target_y;
        goal_msg.pose.position.z = target_z;
        simple_goal_pub_.publish(goal_msg);
        last_pub_tracking_time = ros::Time::now();
        
    }
    if(!planned_update_flag)
    {
        set_cruise_state();
    }
    else
    {
        position_pid_control(planned_point,current_point,max_vel_beishu+0.5,planned_yaw_angle,0,true);
        // vel_command_send(velocity_expected, attitude_expect.z);
        vel_command_send(velocity_expected, 0);
    }
    planned_update_flag = false;
}

